// BlobDetect.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

using namespace std;
using namespace cv;

// Reference: https://2019.robotix.in/tutorial/imageprocessing/blob_detection/
struct BLOBPOINT
{
	int x, y;
};

struct BLOB
{
	int min_x, max_x;
	int min_y, max_y;
	int cen_x, cen_y;
	int n_pixels;
	int ID;
};

int MinR=213, MaxR=255, MinG=4, MaxG=64, MinB=23, MaxB=82;

FILE *csvFile;
struct BLOB *Blob;
int bound[3];
unsigned char minb[4], maxb[4], ming[4], maxg[4];
int TargetX, TargetY;
Mat Image;
int BlobNum;

int MinSize=20, MaxSize=111, MinWidth=5, MaxWidth=16, MinHeight=3, MaxHeight=6;

int OffsetX[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int OffsetY[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int OffsetNum[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int MouseX[4] = {47, 52, 59, 55};
int MouseY[4] = {45, 58, 86, 94};

int BlobID;

/*
int MaxRGB[3][2], MinRGB[3][2];


//int MinWidth=9999, MaxWidth=0, MinHeight=9999, MaxHeight=0, MinSize=9999, MaxSize=0;
//int MinSize=25, MaxSize=115, MinWidth=5, MaxWidth=17, MinHeight=3, MaxHeight=6;

int OffsetSkip = 0;

int MouseX[4] = {52, 51, 59, 54};
int MouseY[4] = {51, 58, 93, 98};
*/






void TargetOffset()
{
	int i;
	int maxsize, centery, id, minx, miny;

	// find max blob
	maxsize = 0;
	BlobID = -1;
	for (i=0; i<BlobNum; i++) {
		if (maxsize < ((Blob+i)->n_pixels)) {
			maxsize = (Blob+i)->n_pixels;
			centery = (Blob+i)->cen_y;
			minx = (Blob+i)->min_x;
			miny = (Blob+i)->min_y;
			id = i;
			BlobID = i;
		}
	}  // for i

	// search the most left blob
	for (i=0; i<BlobNum; i++) {
		if (i == id)
			continue;
		if ( (abs(centery - (Blob+i)->cen_y) <= 1) && (minx > (Blob+i)->min_x) ) {
			minx = (Blob+i)->min_x;
			miny = (Blob+i)->min_y;
			BlobID = i;
		}
	}  // for i

	if (BlobID != -1) {
		i = (Blob+BlobID)->max_y - (Blob+BlobID)->min_y;
		line(Image, Point(TargetX-3, TargetY), Point(TargetX+3, TargetY), Scalar(0, 255, 0));
		line(Image, Point(TargetX, TargetY-3), Point(TargetX, TargetY+3), Scalar(0, 255, 0));
		rectangle(Image, Point((Blob+BlobID)->min_x, (Blob+BlobID)->min_y), Point((Blob+BlobID)->max_x, (Blob+BlobID)->max_y), Scalar(255, 0, 0));
		circle(Image, Point((Blob+BlobID)->min_x+MouseX[i-MinHeight], (Blob+BlobID)->min_y+MouseY[i-MinHeight]), 6, Scalar(255,0,0));  // confirm MinHeight first
		printf("      target %d,%d | blob %d,%d | dist %d,%d | size %d,%d | mouse %d %d\n", TargetX, TargetY, (Blob+BlobID)->min_x, (Blob+BlobID)->min_y, TargetX - (Blob+BlobID)->min_x, TargetY - (Blob+BlobID)->min_y, (Blob+BlobID)->max_x - (Blob+BlobID)->min_x, (Blob+BlobID)->max_y - (Blob+BlobID)->min_y, MouseX[i-MinHeight], MouseY[i-MinHeight]);
		imshow("Image", Image);
		//waitKey(0);
	}
}

void GetBlobs(Mat img, int slotx[8], int sloty[8], int slotnum, char *filename, int mode)
{
	int i, j, k, l, row, col, id;
	int *pixel_ID;
	struct BLOBPOINT *openlist;
	uchar *data, *data2;
	int head=0, tail=0;

	BlobNum = 0;
	row = img.rows;
	col = img.cols;
	id = 1;
	pixel_ID = (int *)malloc(sizeof(int) * row * col);
	openlist = (struct BLOBPOINT*)malloc(sizeof(struct BLOBPOINT) * row * col);

	for (i=0; i<row; i++) {
		for (j=0; j<col; j++)
			*(pixel_ID + i*col + j) = -1;
	}

	for (i=1; i<row-1; i++) {
		data = img.ptr<uchar>(i);
		for (j=1; j<col-1; j++) {
			if (data[j] == 0 || *(pixel_ID + i*col + j) > -1)
			//if (img.at<uchar>(i,j) == 0 || *(pixel_ID + i*col + j) > -1)
				continue;

			(openlist + tail)->x = j;
			(openlist + tail)->y = i;
			tail++;
			int sum_x=0, sum_y=0, n_pixels=0, max_x=0, max_y=0;
			int min_x=col+1, min_y=row+1;
			while (tail > head) {
				//Dequeue the element at the head of the queue
				struct BLOBPOINT top;
				top.x = (openlist + head)->x;
				top.y = (openlist + head)->y;
				head++;
				*(pixel_ID + top.y*col + top.x) = id;
				n_pixels++;

				//To obtain the bounding box of the blob w.r.t the original image
				min_x = (top.x<min_x) ? top.x : min_x;
				min_y = (top.y<min_y) ? top.y : min_y;
				max_x = (top.x>max_x) ? top.x : max_x;
				max_y = (top.y>max_y) ? top.y : max_y;
				sum_y += top.y;
				sum_x += top.x;

				//Add the 8-connected neighbours that are yet to be visited, to the queue
				for (k=top.y-1; k<=top.y+1; k++) {
					data2 = img.ptr<uchar>(k);
					for (l=top.x-1; l<=top.x+1; l++) {
						if (data2[l] == 0 || *(pixel_ID + k*col + l) > -1)
						//if (img.at<uchar>(k,l) == 0 || *(pixel_ID + k*col + l) > -1)
							continue;
						*(pixel_ID + k*col + l) = id;
						(openlist + tail)->x = l;
						(openlist + tail)->y = k;
						tail++;
					}
				}
			}

			if (mode == 2 && (BlobNum>=1000 || n_pixels < MinSize || n_pixels > MaxSize || max_x-min_x < MinWidth || max_x-min_x>MaxWidth || max_y-min_y < MinHeight || max_y-min_y > MaxHeight))  continue;

			(Blob + BlobNum)->min_x = min_x;
			(Blob + BlobNum)->max_x = max_x;
			(Blob + BlobNum)->min_y = min_y;
			(Blob + BlobNum)->max_y = max_y;
			(Blob + BlobNum)->cen_x = sum_x / n_pixels;
			(Blob + BlobNum)->cen_y = sum_y / n_pixels;
			(Blob + BlobNum)->n_pixels = n_pixels;
			(Blob + BlobNum)->ID = id;
			BlobNum++;
			id++;
		}  // for j
	}  // for i

	// draw blobs detected
	printf("    blob num: %d\n", BlobNum);
	Mat blobimg(img.rows, img.cols, CV_8U, Scalar(0));
	for (i=0; i<BlobNum; i++) {
		k = 0;
		for (j=0; j<slotnum; j++) {
			if (slotx[j] >= (Blob+i)->min_x && slotx[j] <= (Blob+i)->max_x  && sloty[j] >= (Blob+i)->min_y && sloty[j] <= (Blob+i)->max_y) {
				if (mode == 1)
					fprintf(csvFile, "%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", filename+24, slotx[j], sloty[j], (Blob+i)->cen_x, (Blob+i)->cen_y, (Blob+i)->min_x, (Blob+i)->min_y, (Blob+i)->max_x, (Blob+i)->max_y, (Blob+i)->n_pixels, (Blob+i)->max_x-(Blob+i)->min_x, (Blob+i)->max_y-(Blob+i)->min_y);
				k = 1;
				if (j == 0) {
					OffsetX[(Blob+i)->max_y-(Blob+i)->min_y] += TargetX - (Blob+i)->min_x;
					OffsetY[(Blob+i)->max_y-(Blob+i)->min_y] += TargetY - (Blob+i)->min_y;
					OffsetNum[(Blob+i)->max_y-(Blob+i)->min_y]++;
				}
				break;
			}
		}  // for j

		rectangle(blobimg, Point((Blob + i)->min_x,(Blob + i)->min_y), Point((Blob + i)->max_x,(Blob + i)->max_y), Scalar(127+k*128), -1, 4);  // blob
		if (k == 1)
			printf("  *%d: %d %d | %d %d %d %d | %d\n", (Blob + i)->ID, (Blob + i)->cen_x, (Blob + i)->cen_y, (Blob + i)->max_x, (Blob + i)->max_y, (Blob + i)->min_x, (Blob + i)->min_y, (Blob + i)->n_pixels);
		else
			printf("  %d: %d %d | %d %d %d %d | %d\n", (Blob + i)->ID, (Blob + i)->cen_x, (Blob + i)->cen_y, (Blob + i)->max_x, (Blob + i)->max_y, (Blob + i)->min_x, (Blob + i)->min_y, (Blob + i)->n_pixels);
	}  // for i
	imshow("Blobs", blobimg);
	
	free(pixel_ID);
	free(openlist);
}

void BlobDetect(char *filename, int slotx[8], int sloty[8], int slotnum, int mode)
{
	int i, j;
	uchar *data_rgb, *data_gray;
	unsigned char r, g, b;

	Image = imread(filename);
	Mat gray(Image.rows, Image.cols, CV_8U, Scalar(0));

	// extract the pixel by color
	for (j=0; j<Image.rows; j++) {
		data_rgb = Image.ptr<uchar>(j);
		data_gray = gray.ptr<uchar>(j);
		for (i=0; i<Image.cols; i++) {
			b = data_rgb[i*3];
			g = data_rgb[i*3+1];
			r = data_rgb[i*3+2];
			if ( (r >= MinR    && r <= bound[0] && g >= ming[0] && g <= maxg[0] && b >= minb[0] && b <= maxb[0]) || 
				 (r > bound[0] && r <= bound[1] && g >= ming[1] && g <= maxg[1] && b >= minb[1] && b <= maxb[1]) || 
				 (r > bound[1] && r <= bound[2] && g >= ming[2] && g <= maxg[2] && b >= minb[2] && b <= maxb[2]) || 
				 (r > bound[2] && r <= MaxR     && g >= ming[3] && g <= maxg[3] && b >= minb[3] && b <= maxb[3]) ) {
				data_gray[i] = 255;
			}
		}  // for i
	}  // for j

	imshow("Image", Image);
	imshow("Gray", gray);

	GetBlobs(gray, slotx, sloty, slotnum, filename, mode);
	//waitKey(0);
}

int _tmain(int argc, _TCHAR* argv[])
{
	FILE *pFile;
	char str[1024], filename[80];
	char *pch;
	int x[8], y[8], num, i;

	bound[0]=218; bound[1]=228; bound[2]=239;
	ming[0]=4; maxg[0]=14; minb[0]=23; maxb[0]=41;
	ming[1]=6; maxg[1]=56; minb[1]=24; maxb[1]=82;
	ming[2]=12; maxg[2]=49; minb[2]=26; maxb[2]=81;
	ming[3]=19; maxg[3]=64; minb[3]=28; maxb[3]=82;


	// first part: get blol information
	/*
	Blob = (struct BLOB*)malloc(sizeof(struct BLOB) * 1000);

	pFile = fopen ("..\\MarkingSlot\\Mark.csv" , "r");
	csvFile = fopen ("Blob.csv" , "w");
	if (pFile == NULL || csvFile == NULL) {
		printf("Cannot open ..\\MarkingSlot\\Mark.csv or Blob.csv\n");
		return -1;
	}

	while (fgets(str, 1024, pFile) != NULL) {
		printf("%s", str);
		num = 0;
		pch = strtok(str," ,\n");
		while (pch != NULL) {
			// first token: file name
			if (num == 0) {
				if (pch[0] == '*') {
					strcpy(filename, pch+1);
				}
				else {
					strcpy(filename, pch);
				}
			}
			else {
				if (num % 2 == 1) {
					if (pch[0] >= '0' && pch[0] <= '9')
						x[(num-1)/2] = atoi(pch);
					else
						TargetX = atoi(pch+1);
				}
				else {
					if (pch[0] >= '0' && pch[0] <= '9')
						y[(num-1)/2] = atoi(pch);
					else
						TargetY = atoi(pch+1);
				}
			}
			num++;
			pch = strtok (NULL, " ,\n");
		}  // while pch
		printf("    Marked Blob Number %d:  ", num/2-1);
		for (i=0; i<num/2-1; i++)
			printf("%d,%d  ", x[i], y[i]);
		printf("\n");

		BlobDetect(filename, x, y, num, 1);
	}  // while fgets

	printf("\nint MouseX[12] = {");
	for (i=0; i<12; i++) {
		if (OffsetNum[i] != 0)
			printf("%d", (int)((OffsetX[i]*1.0 / OffsetNum[i]) + 0.5));
		else
			printf("0");
		if (i < 11)
			printf(", ");
		else
			printf("};\n");
	}
	printf("int MouseY[12] = {");
	for (i=0; i<12; i++) {
		if (OffsetNum[i] != 0)
			printf("%d", (int)((OffsetY[i]*1.0 / OffsetNum[i]) + 0.5));
		else
			printf("0");
		if (i < 11)
			printf(", ");
		else
			printf("};\n");
	}
	printf("\nint SlotNum[12] = {%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d};\n", OffsetNum[0], OffsetNum[1], OffsetNum[2], OffsetNum[3], OffsetNum[4], OffsetNum[5], OffsetNum[6], OffsetNum[7], OffsetNum[8], OffsetNum[9], OffsetNum[10], OffsetNum[11]);
	
	fclose(pFile);
	fclose(csvFile);
	free(Blob);
	*/

	// second part: evaluate result

	Blob = (struct BLOB*)malloc(sizeof(struct BLOB) * 1000);

	pFile = fopen ("..\\MarkingSlot\\Mark.csv" , "r");
	if (pFile == NULL) {
		printf("Cannot open ..\\MarkingSlot\\Mark.csv\n");
		return -1;
	}

	while (fgets(str, 1024, pFile) != NULL) {
		printf("%s", str);
		num = 0;
		pch = strtok(str," ,\n");
		while (pch != NULL) {
			// first token: file name
			if (num == 0) {
				if (pch[0] == '*') {
					strcpy(filename, pch+1);
				}
				else {
					strcpy(filename, pch);
				}
			}
			else {
				if (num % 2 == 1) {
					if (pch[0] >= '0' && pch[0] <= '9')
						x[(num-1)/2] = atoi(pch);
					else
						TargetX = atoi(pch+1);
				}
				else {
					if (pch[0] >= '0' && pch[0] <= '9')
						y[(num-1)/2] = atoi(pch);
					else
						TargetY = atoi(pch+1);
				}
			}
			num++;
			pch = strtok (NULL, " ,\n");
		}  // while pch
		printf("    Marked Blob Number %d:  ", num/2-1);
		for (i=0; i<num/2-1; i++)
			printf("%d,%d  ", x[i], y[i]);
		printf("\n");

		BlobDetect(filename, x, y, num, 2);
		TargetOffset();
		waitKey(0);
	}  // while fgets

	
	fclose(pFile);
	free(Blob);

	return 0;
}

